using System;
using System.Collections.Generic;
using System.Linq;
using MagistrAPI.Enums;
using MagistrAPI.MathModel;

namespace MagistrAPI.Integration
{
    public class RK4Integrator : IIntegrator
    {
        private readonly Equations[] _equations = {
                                                          Equations.X, Equations.Y, Equations.Z,
                                                          Equations.Vx,
                                                          Equations.Vy, Equations.Vz, Equations.Wx,
                                                          Equations.Wy,
                                                          Equations.Wz, Equations.Lx, Equations.Ly,
                                                          Equations.Lz,
                                                          Equations.Mx, Equations.My, Equations.Mz,
                                                          Equations.Nx,
                                                          Equations.Ny, Equations.Nz
                                                      };
        private void Correct(ref double lx, ref double ly, ref double lz, ref double mx, ref double my, 
                                ref double mz,ref double nx, ref double ny, ref double nz, int n, double e)
        {
            double lxx = lx,
                   lyy = ly,
                   lzz = lz,
                   mxx = mx,
                   myy = my,
                   mzz = mz,
                   nxx = 0,
                   nyy = 0,
                   nzz = 0;
            double ALx1, ALx2, ALx3, ALy1, ALy2, ALy3, R1, R2, R3, Al1, Al2, Al3, Am1, Am2, Am3;
            var i = 0;
            while (i < n)
            {
                ALx1 = Math.Sqrt(lxx * lxx + lyy * lyy + lzz * lzz);
                ALx2 = Math.Sqrt(mxx * mxx + myy * myy + mzz * mzz);
                ALx3 = lxx * mxx + lyy * myy + lzz * mzz;
                ALy1 = Math.Abs(1 - ALx1);
                ALy2 = Math.Abs(1 - ALx2);
                ALy3 = Math.Abs(ALx3);
                if ((ALy1 <= e) && (ALy2 <= e) && (ALy3 <= e))
                {
                    i = n - 1;
                }
                R1 = (1 + ALx1) / (2 * ALx1);
                R2 = (1 + ALx2) / (2 * ALx2);
                R3 = ALx3 / (ALx1 + ALx2);
                Al1 = R1 * lxx - mxx * R3;
                Al2 = R1 * lyy - myy * R3;
                Al3 = R1 * lzz - mzz * R3;
                Am1 = R2 * mxx - lxx * R3;
                Am2 = R2 * myy - lyy * R3;
                Am3 = R2 * mzz - lzz * R3;
                lxx = Al1;
                lyy = Al2;
                lzz = Al3;
                mxx = Am1;
                myy = Am2;
                mzz = Am3;
                nxx = Al2 * Am3 - Al3 * Am2;
                nyy = Al3 * Am1 - Al1 * Am3;
                nzz = Al1 * Am2 - Al2 * Am1;
                i++;
            }
            lx = lxx;
            ly = lyy;
            lz = lzz;
            mx = mxx;
            my = myy;
            mz = mzz;
            nx = nxx;
            ny = nyy;
            nz = nzz;
        }

        public Dictionary<Equations, List<double>> Integrate(int t0, int tN, double h0, double hMax, double d, double[] y0)
        {
            var y = Initialize(y0);
            var n = y.Length;
            var h = h0;
            var rezult = _equations.ToDictionary(key => key, key => new List<double>());
            rezult.Add(Equations.T, new List<double>());
            for (var i = 0; i < n; i++)
            {
                rezult.Values.ElementAt(i).Add(y[i]);
            }
            rezult.Values.ElementAt(18).Add(0.0);
            var t = 0.0;
            while (MathModelFunc.H(y[0], y[1], y[2]) > 0)
            {
                Correct(ref y[9], ref y[10], ref y[11], ref y[12], ref y[13], ref y[14], ref y[15], ref y[16], ref y[17],
                        30,
                        1e-10);
                t += h;
                var k1 = NextStep(y);
                for (var i = 0; i < n; i++)
                {
                    y[i] += k1[i]*h*0.5;
                }
                var k2 = NextStep(y);
                for (var i = 0; i < n; i++)
                {
                    y[i] += k2[i]*h*0.5;
                }
                var k3 = NextStep(y);
                for (var i = 0; i < n; i++)
                {
                    y[i] += k3[i]*h;
                }
                var k4 = NextStep(y);
                var dS = new double[n];
                for (var i = 0; i < n; i++)
                {
                    dS[i] = Math.Abs(h*(k1[i] - k2[i] - k3[i] + k4[i])/d);
                }
                var dSMax = dS.Max();
                if (dSMax <= 1)
                {
                    for (var i = 0; i < n; i++)
                    {
                        y[i] += (k1[i] + 2*k2[i] + 2*k3[i] + k4[i])*(h/6.0);
                        rezult.Values.ElementAt(i).Add(y[i]);
                    }
                    rezult.Values.ElementAt(18).Add(t);
                    if (dSMax < 0.1)
                    {
                        h *= 2;
                    }
                    if (h>hMax)
                    {
                        h = hMax;
                    }
                }
                else
                {
                    h /= 2;
                }
            }
            return rezult;
        }

        private double [] NextStep(double [] y)
        {
            var n = y.Length;
            var kn = new double[n];
            var i = 0;
            foreach (var equation in _equations.Select(equationEnum => (Func<double[], double>) SystemOfEquations.Equations[equationEnum]))
            {
                kn[i] = equation(y);
                i++;
            }
            return kn;
        }

        private double[] Initialize(double[] y0)
        {
            if (y0.Length != 12)
            {
                throw new ArgumentException();
            }
            double x = y0[0];
            double y = y0[1];
            double z = y0[2];
            double vX = y0[3];
            double vY = y0[4];
            double vZ = y0[5];
            double wX = y0[6];
            double wY = y0[7];
            double wZ = y0[8];
            double fi = y0[9];
            double al = y0[10];
            double gam = y0[11];
            double[,] lugl0 = MathModelFunc.LugL0(x, y, z, vX, vY, vZ, fi, al, gam);
            return new[]
                       {
                           x, y, z, vX, vY, vZ, wX, wY, wZ, lugl0[0, 0], lugl0[1, 0], lugl0[2, 0], lugl0[0, 1],
                           lugl0[1, 1]
                           , lugl0[2, 1], lugl0[0, 2], lugl0[1, 2], lugl0[2, 2]
                       };
        }
    }
}